/*
 * mark.cpp
 *
 *  Created on: 13.06.2011
 *      Author: nick
 */

#include "mark.h"

// Create a mark with the parameters of a blob
CMark::CMark( CBlob* blob )
	: m_roundness(), m_area(blob->area), m_perimeter(blob->perimeter),
	  m_vector(),  m_pos(), m_orientation(), m_updateCounter(false), m_tracked(false), m_latestFrame()
{
	//compute roundness
	m_roundness = pow(m_perimeter,2) / m_area;

	//compute position
	m_pos.x = blob->maxx - (blob->maxx - blob->minx)/2;
	m_pos.y = blob->maxy - (blob->maxy - blob->miny)/2;

	//compute orientation
	if(m_pos.x > (X_WIDTH - 2*X_WIDTH/5) )
		m_orientation = rightWall;
	else if( ( 2*X_WIDTH/5) )
		m_orientation = leftWall;
	else
		m_orientation = middle;
}

void CMark::printData() const
{
	cout 	<< "\nPosition( " << m_pos.x << " / " << m_pos.y << " )\n"
							<< "Roundness: " << m_roundness << "\n"
							<< "Area: " << m_area << "\n";
}

// returns the roundness of the mark
double CMark::roundness() const
{ return m_roundness; }

// returns the area of the mark
double CMark::area() const
{ return m_area; }

// vector of movement
SVec2D CMark::vector() const
{ return m_vector; }

// centerposition of the mark
SVec2D CMark::pos() const
{ return m_pos; }

// perimeter of the mark
double CMark::perimeter() const
{ return m_perimeter; }

// orientation of the mark
EOrientation CMark::orientation() const
{ return m_orientation; }

// returns true if the mark has been tracked
bool CMark::tracked() const
{ return m_tracked; }

//! returns the last frame the Mark has been tracked on
Mat CMark::lastFrame()
{ return m_latestFrame; }

// returns true if last position has been near the border of the fov
bool CMark::lastSeenOnBorder()
{
	switch(m_orientation)
	{
		case leftWall:
			if( m_pos.x < BORDER_AREA )
				return true;
			else
				return false;
		case rightWall:
			if( m_pos.x > X_WIDTH - BORDER_AREA )
				return true;
			else
				return false;
		case middle:
			return false;
	}
	return false; //get rid of warning
}

//! draws the Markdata into it's latest frame
void CMark::drawDataToLatestFrame()
{ drawDataToMat( m_latestFrame, true  ); }

/*
	 * compare another mark to this one. Returns a value
	 * between 0-100 that indicates the probability of
	 * the two Marks being the same.
	 */
int CMark::equals( CMark* oldMark) const
{
	int similarity = 0;

	if(oldMark == NULL)
		return 0;

	//compare different features
	//orientation
	if(oldMark->orientation() == m_orientation)
		similarity += ORIENTATION_BONUS;

	//roundness (smaller value means more round) -> factor should be < 1)
	double roundnessFactor = oldMark->roundness() / m_roundness;
	if(roundnessFactor < (1 + ROUNDNESS_DELTA) )
		similarity += ROUNDNESS_BONUS;

	//area (should be growing -> factor > 1
	double areaFactor = m_area/oldMark->area() ;
	if( areaFactor > (1 - AREA_DELTA) )
		similarity += AREA_BONUS;

	//movement vector
#if 0
	int dx = m_vector.x/oldMark->vector().x;
	int dy = m_vector.y/oldMark->vector().y;

	cout << "vector(" << m_vector.x << "/" << m_vector.y << "\n";
	cout << "dx/dy(" << dx << "/" << dy << "\n";

	//	if( ( (dx < 2) | (dx > 0.5) ) && ( (dy < 2) | (dy > 0.5) ) )
		//	similarity += VECTOR_BONUS;
#endif


	float xdiff =  oldMark->pos().x - m_pos.x;
	xdiff = abs(xdiff);

	if(xdiff < MAX_XDIFF)
		similarity += VECTOR_BONUS;

	if(similarity < 0)
		similarity = 0;
	return similarity;
}

void CMark::updateData(CMark* newMark, Mat frame)
{
	if(newMark == NULL)
		return;

	frame.copyTo(m_latestFrame);

	m_tracked = true;
	m_updateCounter = 0;
	m_area = newMark->area();
	m_roundness = newMark->roundness();

	m_vector.x = m_pos.x - newMark->pos().x;
	m_vector.y = m_pos.y - newMark->pos().y;
#if 0
	cout << "vector(" << m_vector.x << "/" << m_vector.y << "\n";
#endif
	// IMPORTANT: calculate vector BEFORE setting the new position
	m_pos = newMark->pos();
	m_perimeter = newMark->perimeter();
}

//! draw the data of the mark into a mat
void CMark::drawDataToMat(Mat &Mat, bool textInfo=false) const
{
	Point2d p1 = Point2d( m_pos.x, m_pos.y );			//line 1
	Point2d p2 = Point2d( m_pos.x, m_pos.y+14 );	//line 2
	Point2d p3 = Point2d( m_pos.x, m_pos.y+28 );	//line 3
	//vector
	Point2d p4 = Point2d( m_pos.x - m_vector.x , m_pos.y - m_vector.y );
	//center cross
	Point2d pc1 = Point2d( m_pos.x + 5, m_pos.y );
	Point2d pc2 = Point2d( m_pos.x - 5, m_pos.y );
	Point2d pc3 = Point2d( m_pos.x, m_pos.y + 5 );
	Point2d pc4 = Point2d( m_pos.x, m_pos.y - 5);

	line(Mat, pc1 ,pc2, white, 1 , 1 , 0 );
	line(Mat, pc3 ,pc4, white, 1 , 1 , 0 );

	//marks that are being tracked are shown as green circles
	if(m_tracked){
		circle(Mat, p1, /*sqrt(m_area/3.14)*/ m_perimeter/(2*3.14), green  );
		line(Mat, p1 ,p4, blue, 1 , 4 , 0 );
	}
	else
		circle(Mat, p1, /*sqrt(m_area/3.14)*/ m_perimeter/(2*3.14), blue  );

	string area = intToString((int) m_area );
	string roundness = intToString((int) m_roundness );

	//write text on the mark
	if(textInfo) {
		putText( Mat , area, p1, 1, 0.6, Scalar(0,255,255), 1 , 28 , false );
		putText( Mat , roundness, p2, 1, 0.6, Scalar(255,255,0), 1 , 28 , false );


		if(m_orientation == leftWall)
			putText( Mat , "L" , p3, 1, 0.6, white , 1 , 28 , false );
		else if(m_orientation == rightWall)
			putText( Mat , "R" , p3, 1, 0.6, white , 1 , 28 , false );
		else
			putText( Mat , "N" , p3, 1, 0.6, white , 1 , 28 , false );
	}
}

// returns the value of the updatecounter
int CMark::updateCounter() const
{ return m_updateCounter; }

//! increments the updatecounter
void CMark::incUpdateCounter()
{ m_updateCounter++; }

string intToString(int i)
{
	string s;
	stringstream out;
	out<<i;
	s = out.str();
	return s;
}
